/*
 * bldc_smc.c
 *
 *  Created on: Jun 10, 2025
 *      Author: 21954
 */
#include "bldc_smc.h"
#include "adc.h"



volatile uint8_t timer_flag;
int16_t drag_angle;
uint8_t is_being_started=1;


MOTOR0_PARA motor0 = {  BLDC_CONTROL_FOC_PARA_DEFAULTS,
                        BLDC_CONTROL_SMC_PARA_DEFAULTS,
                        BLDC_CONTROL_SMC_PLL_PARA_DEFAULTS,
                        BLDC_CONTRL_PID_PARA_DEFAULTS,
                        NULL
                    };
BLDC_CONTRL_SPD_PARA        pll_speed_control;

void hpm_mcl_bldc_foc_pwmset(BLDC_CONTROL_PWMOUT_PARA *par)
{
    switch (par->i_motor_id) {
    case BLDC_MOTOR0_INDEX:
    	TIM1->CCR1 = par->pwm_u;
    	TIM1->CCR2 = par->pwm_v;
    	TIM1->CCR3 = par->pwm_w;

#if defined(HPMSOC_HAS_HPMSDK_PWMV2)
        pwmv2_shadow_register_unlock(MOTOR0_BLDCPWM);
        pwmv2_set_shadow_val(MOTOR0_BLDCPWM, BOARD_BLDCPWM_CMP_INDEX_1, (pwm_reload - pwm_u_half), 0, false);
        pwmv2_set_shadow_val(MOTOR0_BLDCPWM, BOARD_BLDCPWM_CMP_INDEX_2, (pwm_reload + pwm_u_half), 0, false);
        pwmv2_set_shadow_val(MOTOR0_BLDCPWM, BOARD_BLDCPWM_CMP_INDEX_3, (pwm_reload - pwm_v_half), 0, false);
        pwmv2_set_shadow_val(MOTOR0_BLDCPWM, BOARD_BLDCPWM_CMP_INDEX_4, (pwm_reload + pwm_v_half), 0, false);
        pwmv2_set_shadow_val(MOTOR0_BLDCPWM, BOARD_BLDCPWM_CMP_INDEX_5, (pwm_reload - pwm_w_half), 0, false);
        pwmv2_set_shadow_val(MOTOR0_BLDCPWM, BOARD_BLDCPWM_CMP_INDEX_6, (pwm_reload + pwm_w_half), 0, false);
        pwmv2_shadow_register_lock(MOTOR0_BLDCPWM);
#endif
        break;

    default:
        break;
    }
}
void motor0_current_loop(float angle)
{
    uint8_t smc_en;

    smc_en = !is_being_started;
    motor0.foc_para.samplcurpar.adc_u = (hadc1.Instance->JDR1 * 0.029373f - 75.0f);
    motor0.foc_para.samplcurpar.adc_v = (hadc1.Instance->JDR2 * 0.029373f - 75.0f);
    motor0.foc_para.electric_angle = angle;
    motor0.foc_para.func_dqsvpwm(&motor0.foc_para, &motor0.smc_para, &motor0.smc_pll, &smc_en);
    motor0.foc_para.pwmpar.pwmout.func_set_pwm(&motor0.foc_para.pwmpar.pwmout);
}
void motor0_highspeed_loop(void)
{
    motor0_current_loop(drag_angle);
    pll_speed_control.speedtheta = motor0.smc_pll.theta;
//    pll_speed_control.func_getspd(&pll_speed_control);
}

void set_adval_middle(void)
{
    float adc_u_sum = 0;
    float adc_v_sum = 0;
    uint16_t times = 0;

    motor0.foc_para.pwmpar.pwmout.pwm_u = PWM_RELOAD >> 1;
    motor0.foc_para.pwmpar.pwmout.pwm_v = PWM_RELOAD >> 1;
    motor0.foc_para.pwmpar.pwmout.pwm_w = PWM_RELOAD >> 1;
    motor0.foc_para.pwmpar.pwmout.func_set_pwm(&motor0.foc_para.pwmpar.pwmout);
    do {
        if (timer_flag == 1) {
            adc_u_sum += (hadc1.Instance->JDR1 * 0.029373f - 75.0f);//3.3/4096*36.458333=0.029373,2.057143f * 36.458333f=75.0f
            adc_v_sum += (hadc1.Instance->JDR2 * 0.029373f - 75.0f);
            times++;
            if (times >= BLDC_CURRENT_SET_TIME_MS) {
                break;
            }
            timer_flag = 0;
        }
    } while (1);
    motor0.foc_para.samplcurpar.adc_u_middle = adc_u_sum / BLDC_CURRENT_SET_TIME_MS;
    motor0.foc_para.samplcurpar.adc_v_middle = adc_v_sum / BLDC_CURRENT_SET_TIME_MS;
}

void bldc_init_par(void)
{
    BLDC_CONTROL_FOC_PARA *par = &motor0.foc_para;
    par->motorpar.func_smc_const = (void(*)(void *))&hpm_mcl_smc_const_cal;
    par->motorpar.i_lstator_h = 0.00086;
    par->motorpar.i_maxspeed_rs = 35;
    par->motorpar.i_phasecur_a = 0.5;
    par->motorpar.i_phasevol_v = 12;
    par->motorpar.i_poles_n = 7;
    par->motorpar.i_rstator_ohm = 2.3;
    par->motorpar.i_samplingper_s = 0.00005;
    par->motorpar.func_smc_const(&par->motorpar);

    pll_speed_control.i_speedacq = 20;
    pll_speed_control.i_speedfilter = 0.0075;
    pll_speed_control.i_speedlooptime_s = 0.00005*20;
    pll_speed_control.i_motorpar = &par->motorpar;
    pll_speed_control.func_getspd = (void(*)(void *))&hpm_mcl_bldc_foc_al_speed;

    par->currentdpipar.i_kp = 0.5;
    par->currentdpipar.i_ki = 0.0;
    par->currentdpipar.i_max = 6000;
    par->currentdpipar.func_pid = (void(*)(void *))&hpm_mcl_bldc_foc_pi_contrl;

    par->currentqpipar.i_kp = 0.5;
    par->currentqpipar.i_ki = 0.0;
    par->currentqpipar.i_max = 6000;
    par->currentqpipar.func_pid = (void(*)(void *))&hpm_mcl_bldc_foc_pi_contrl;

    par->pwmpar.func_spwm = (void(*)(void *))&hpm_mcl_bldc_foc_svpwm;
    par->pwmpar.i_pwm_reload_max = PWM_RELOAD*0.95;
    par->pwmpar.pwmout.func_set_pwm = (void(*)(void *))&hpm_mcl_bldc_foc_pwmset;
    par->pwmpar.pwmout.i_pwm_reload = PWM_RELOAD;
    par->pwmpar.pwmout.i_motor_id = BLDC_MOTOR0_INDEX;

    par->samplcurpar.func_sampl = (void(*)(void *))&hpm_mcl_bldc_foc_current_cal;
    par->func_dqsvpwm =  (void (*)(void *, void *, void *, void *))hpm_mcl_smc_loop;

    motor0.speedloop_para.func_pid  = (void(*)(void *))hpm_mcl_bldc_foc_pi_contrl;
    motor0.speedloop_para.i_kp      = 1.0;
    motor0.speedloop_para.i_ki      = 0.095;
    motor0.speedloop_para.i_max     = 3500;

    motor0.adc_trig_event_callback = &motor0_highspeed_loop;

    motor0.smc_para.zero = 0.05;
    motor0.smc_para.ksmc = 160;
    motor0.smc_para.filter_coeff  = 0.05;
    motor0.smc_para.i_motorpar = &par->motorpar;
    motor0.smc_para.ualpha = &par->ualpha;
    motor0.smc_para.ubeta = &par->ubeta;
    motor0.smc_para.ialpha = &par->ialpha;
    motor0.smc_para.ibeta = &par->ibeta;
    motor0.smc_para.func_smc = (void(*)(void *))hpm_mcl_smc_pos_cal;
    motor0.smc_pll.kp = 0.01;
    motor0.smc_pll.ki = 0.005;
    motor0.smc_pll.loop_in_sec = 0.00005;
    motor0.smc_pll.theta0 = 0;//90
    motor0.smc_pll.max_i = 5000;
    motor0.smc_pll.min_i = -5000;
    motor0.smc_pll.max_o = 5000;
    motor0.smc_pll.min_o = -5000;
    par->pos_estimator_par.func = motor0.smc_para.func_smc;
    par->pos_estimator_par.par = &motor0.smc_para;
}

//void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
//{
//	motor0.adc_trig_event_callback();
//}
